Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

AP_InertialSensor: Rework rotation handling #5216

Closed
wants to merge 1 commit into from

Conversation

jberaud
Copy link
Contributor

@jberaud jberaud commented Nov 15, 2016

Some rotations were applied on the raw data from the sensors in an implicit way. This PR solves this as described after. It makes the integration on a new board a bit more straightforward and the drivers get a bit easier to understand.
Tested on
Bebop
Disco
Pixhawk 1
pixhawk2, pixracer, pixfalcon, phmini raspilot (by Tridge)

so it needs to be carefully reviewed and tested on other boards.

Right now, MPU based imus perform the rotations in the driver in an
implicit way when converting sensor bus data to actual x, y, z data.
MPU6000 and MPU9250 :
x = y, y = x, z = -z
meaning ROTATION_ROLL_180_YAW_90
LSM9DS0 and L3G4200 :
x = x, y = -y, z = -z
meaning ROTATION ROLL_180

This commit removes these implict rotations from the drivers and modify the
per-board orientations so the behaviour doesn't change, by applying a
combination of ROLL_180_YAW_90 and then the former board rotation for MPU9250
and MPU6000 and applying a combination of ROLL_180 and the former board rotation
for LSM9DS0 and L3G4200.

RASPILOT had a compilation flag inside the LSM9DS0
driver to add a YAW_90 rotation. When added to the ROLL_180 applied when
converting data, it gives the same rotation as the computed rotation for the
mpu6000, i.e ROLL_180_YAW_90

The rotation combinations I have calculated for reference:
Applying ROTATION_ROLL_180_YAW_90:
then ROTATION_PITCH_180 => ROTATION_YAW_90
then ROTATION_ROLL_180_YAW_90 => ROTATION_NONE
then ROTATION_ROLL_180_YAW_270 => ROTATION_YAW_180
then ROTATION_YAW_270 => ROTATION_ROLL_180
then ROTATION_YAW_90 => ROTATION_PITCH_180
then ROTATION_ROLL_180 => ROTATION_YAW_270
then ROTATION_PITCH_180_YAW_90 => ROTATION_YAW_180

@tridge
Copy link
Contributor

tridge commented Nov 15, 2016

hi Julien, this is a nice re-work, but it will need a lot of testing on different boards. I did a spot-check on a Pixhawk1 and found the 2nd accel ends up with the wrong rotation. The PR changes the LSM9DS0 driver gyro orientation but not the accel orientation. I think others are wrong too.

@tridge tridge self-assigned this Nov 16, 2016
@tridge
Copy link
Contributor

tridge commented Nov 16, 2016

I just checked master and it looks like we have an existing error in the l3gd20 gyro orientation from my sensor re-work. I'll work on fixing that now.

@tridge
Copy link
Contributor

tridge commented Nov 16, 2016

i've fixed the l3gd20 rotation, and removed the raspilot hack in the driver. It turns out we need a different rotation for the gyro than for the accel. I had assumed they were the same in my recent changes.
I'd like to get the rest of your changes in, we just need to ensure it keeps the rotations right.

@jberaud
Copy link
Contributor Author

jberaud commented Nov 16, 2016

@tridge I don't think that it is possible that we need a different rotation for the accels and the gyros.
Ok. I'll rework that.

@jberaud
Copy link
Contributor Author

jberaud commented Nov 16, 2016

fixed the rotation for accel in L3G and LSM drivers.

@jberaud jberaud force-pushed the pr/imu-orient branch 2 times, most recently from 3560066 to 95e247f Compare November 16, 2016 13:45
@jberaud
Copy link
Contributor Author

jberaud commented Nov 16, 2016

Rebased. Tested OK on pixhawk.
So tested on Bebop Disco and Pixhawk.

@tridge
Copy link
Contributor

tridge commented Nov 18, 2016

I've tested pixhawk2, pixracer, pixfalcon, phmini raspilot. There were a number of fixes needed:
https://github.com/tridge/ardupilot/commits/pr-julien-orient
It looks to me like others are wrong too. I don't think we can push this in yet. We would need a more systematic way of getting these right.

@jberaud
Copy link
Contributor Author

jberaud commented Nov 18, 2016

Sorry for wasting your time. They are leftovers and a mistake for raspilot. My assumptions regarding rotation changes in the PR message are right.

@jberaud
Copy link
Contributor Author

jberaud commented Nov 18, 2016

@tridge I added your corrections to my commit for clarity purpose. If you prefer, I could just push them on my pr branch with the previous version.

@tridge
Copy link
Contributor

tridge commented Nov 21, 2016

I don't mind either way, just need to do more testing :-)

@jberaud
Copy link
Contributor Author

jberaud commented Jan 2, 2017

@lucasdemarchi You said you could test that PR with some boards. I am waiting some more tests before rebasing. Would you have time for that?

@OXINARF
Copy link
Member

OXINARF commented Jun 5, 2017

@jberaud Is this something you'd still like to work on? It needs a big rebase and I think that if you aren't able to rework it there is no point in keeping it open.

@jberaud
Copy link
Contributor Author

jberaud commented Jun 6, 2017

I can rebase again, but since there are some boards that I don't have, I won't be anymore able to test this PRs on them. So there are 3 solutions :
-> Someone who has these boards tests
-> Someone sends me those boards
-> You can close it. It is not such a big change but it needs testing...

@OXINARF
Copy link
Member

OXINARF commented Jun 6, 2017

I think this is a very worthwhile PR and from comments above I think @tridge does too. I think we can get people to test the boards - coming up with a test procedure for people to follow might help with that.

@lucasdemarchi
Copy link
Contributor

Hi, sorry for the long delay. We do need to test it, but I think getting it mathematically correct would allow us to be more confident on applying it even if we can't test it everywhere.

    x = y, y = x, z = -z
    meaning ROTATION_ROLL_180_YAW_90

This actually means ROTATION_ROLL_180_YAW_270 since it's and intrinsic xyz rotation. You can also check on Vector3::rotate() that what I just said matches the code. Manually checking some of them:

PXF: roll180, yaw270, roll180, yaw270 = yaw180 -> correct on patch
PXFMini: roll180, yaw270, roll180, yaw90 = none -> correct on patch
Bebop: roll180, yaw270, yaw270 = pitch180 -> it's roll180 on patch
Disco: roll180, yaw270, pitch180, yaw90 = yaw180 -> correct on patch
...

The checks above were made manually. I think I had at some point a script using sympy to work out the rotations. If I don't find it I'll derive the rotations again so we can be more confident they are correct.

@jberaud
Copy link
Contributor Author

jberaud commented Jun 8, 2017

@lucasdemarchi Thanks very much for your inputs. Since I have tested that on Bebop, there must be something in my params that I was assuming wrongly. I'll check that, fix the patch, and rebase. Thanks again.

@jberaud
Copy link
Contributor Author

jberaud commented Jun 9, 2017

@lucasdemarchi I have checked the code in vector3 and here is what I see
case ROTATION_ROLL_180_YAW_90: { tmp = x; x = y; y = tmp; z = -z; return; }

Could you explain why it should be ROTATION_ROLL_180_YAW_270 ?

@lucasdemarchi
Copy link
Contributor

Could you explain why it should be ROTATION_ROLL_180_YAW_270

Oh... I always forget the convention we are using... It's zyx, not xyz, i.e. yaw, pitch and roll in order. We need to add a comment on rotations.{h,CPP}. Now to check the others places

@lucasdemarchi
Copy link
Contributor

@jberaud I rechecked all the rotations you changed with the following test program: https://gist.github.com/d049489154f4f90141644b3c90403009

All of them are correct. Could you rebase this?

@jberaud jberaud force-pushed the pr/imu-orient branch 2 times, most recently from 2674f8c to 382e6f3 Compare June 20, 2017 12:17
@jberaud
Copy link
Contributor Author

jberaud commented Jun 20, 2017

@lucasdemarchi Done.

@tridge
Copy link
Contributor

tridge commented Jun 21, 2017

i'm quite nervous about this patch - it is very hard to validate it fully. We thought it was right before and it turned out to be wrong.

@OXINARF
Copy link
Member

OXINARF commented Jun 21, 2017

@jberaud @lucasdemarchi Can we come up with some steps to test this PR? I can bother people to test boards, but without a testing procedure, it is hard.

@jberaud
Copy link
Contributor Author

jberaud commented Jun 28, 2017

@OXINARF Step tests should consist in determining if the orientation of the Accel and Gyro are correct.
So first thing would be to do an accel calibration.

Then to check the gyro orientation, graph gyro (ggyro in mavproxy with mavinit.scr script for primary gyro or ggyro2 for secondary gyro).
Then take the board in hands facing you, meaning what you can see is the front of the board (X axis).
When turning it clockwise, the X line should go negative.

Then take the board in your hand facing its right side (Y axis). Turn it clockwise and the Y axis should go negative.

Then take the board in your hand facing its bottom (Z axis). Turn it clockwise and the Z axis should go negative.

Then to check the accel orientation, graph accel (gaccel for primary and gaccel2 for secondary, etc...).
When the board is on the table (up up and bottom down), Z axis should be negative.

Then turn it and then, put it on its right side (Y axis facing the table). Y should be negative.

Then turn it, put it nose down (X axis facing the table). X should become negative.

Note: When I say "Right" side, I am in the regular drone coordinate system, meaning drone facing straight ahead, not pointing towards you.

@jberaud
Copy link
Contributor Author

jberaud commented Jun 28, 2017

@tridge I understand your concerns. I have seen master being broken numerous times on Bebop but I guess it is because it is quite an uncommon board for ardupilot. I hope we can test this PR correctly enough to merge it, because in the end, it is all about testing right.

Right now, MPU based imus perform the rotations in the driver in an
implicit way when converting sensor bus data to actual x, y, z data.
MPU6000 and MPU9250 :
x = y, y = x, z = -z
meaning ROTATION_ROLL_180_YAW_90
LSM9DS0 and L3G4200 :
x = x, y = -y, z = -z
meaning ROTATION ROLL_180

This commit removes these implict rotations from the drivers and modify
the per-board orientations so the behaviour doesn't change, by applying a
combination of ROLL_180_YAW_90 and then the former board rotation for
MPU9250 and MPU6000 and applying a combination of ROLL_180 and the former board
rotation for LSM9DS0 and L3G4200.

RASPILOT had a compilation flag inside the LSM9DS0 driver to add a YAW_90
rotation. When added to the ROLL_180 applied when converting data,
it gives the same rotation as the computed rotation for the mpu6000,
i.e ROLL_180_YAW_90

Checks: Leftovers and mistakes fixed by tridge included
Tested by lucasdemarchi with
https://gist.github.com/d049489154f4f90141644b3c90403009
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants